#pragma once
#include "ros/ros.h"

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

#include "yaml-cpp/yaml.h"
#include "control/control_params.h"
#include "control/custom_class.h"
#include "common_lib/common.h"
#include "common_lib/log.h"
#include "common_lib/transform.h"
#include "control_msgs/control_msg.h"
#include "control/move_control/path_following_base.h"

#include <tf/tf.h>
namespace control
{
    class CFCPathFollowing: public PathFollowingBase
    {
    public:
        CFCPathFollowing(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
        : stop_distance_threshold_(0.01,0.03)
        {
            Init(nh);
        }
        ~CFCPathFollowing(){}

        void Init(ros::NodeHandle n) override;
		void VehicleController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_speed_limit, float &max_angle_speed_limit) override;
		void ReInit(){running_type_ = RunningType::HEADFOLLOWING;};
		void VehicleStop()
		{
			speed_ = 0;
			angle_speed_ = 0;
			// lqr_speed_smooth_.Reset(g_speed_plan_type);
		};
    private:
        common_lib::Common com_;
        common_lib::Transform tra_;
        common_lib::DoubleThreshold stop_distance_threshold_;

        common_lib::TON ton_switch_type_;
        common_lib::TON ton_zero_speed_;

        amr_trajectory_msgs::point_info ref_point_;

		std::shared_ptr<common_lib::SpeedSmoothBase> ptr_speed_smooth_;
		std::shared_ptr<common_lib::SpeedSmoothBase> ptr_anglespeed_smooth_;

        bool is_in_dec_stage_ = false; // 是否处于减速阶段
		// trajectory_msgs::point_msg last_distance_dec_point_;    // 移动目标的最后一个点，减速阶段是保持不变的
		int last_traj_size_ = 0;                               // 判断路径尺寸，增加一些鲁棒性
		ros::Time last_loop_time_ = ros::Time::now();
        float distance_dec_;
		
        void HeadFollowingController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_speed_limit, float &max_angle_speed_limit);
		void RotatingController(amr_trajectory_msgs::point_info &point, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_angle_speed_limit);		
		void SteerTurningController(amr_trajectory_msgs::point_info &point, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward);
        
        bool IsFollowingToRotating(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, amr_trajectory_msgs::point_info &point);
        bool IsFollowingToSteerturning(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, amr_trajectory_msgs::point_info &point, float speed_toward);
		bool IsRotatingToFollowing(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state);
        bool IsRotatingToSteerTurning(amr_trajectory_msgs::point_info &point, AMRState &amr_state);
        bool IsSteerturningToFollowing(amr_trajectory_msgs::point_info &point, AMRState &amr_state, float speed_toward);

		float TargetLongDistance(AMRState &amr_state,amr_trajectory_msgs::point_info target_point, float line_theta);

        void LineSpeedLimit(AMRState &amr_state, float &speed, float max_speed_limit) override;
    };
}